/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvWorld/ring_queue.h
 * @Description  : 环形队列实现，支持基本的队列操作，包括入队、出队、检查是否为空或已满等。
 *                 该类是线程安全的，使用互斥锁来保护共享数据。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 15:37:55
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#pragma once

#include <assert.h>
#include "z_types.h"
#include "tools.h"


template <typename Type>
class RingQueue
{
 public:
    /* The constructor */
    explicit RingQueue(USHORT size)
                        : size_(size), count_(0), head_pos_(0), tail_pos_(0) {
        queue_ = new Type[size_];
        ASSERT(queue_ != nullptr);
        SecureZeroMemory(queue_, sizeof(Type) * size_);
    }

    /* Default constructor */
    RingQueue() : size_(0), count_(0), head_pos_(0), tail_pos_(0), queue_(nullptr) {}
    
    /* The destructor */
    ~RingQueue() {
        delete[] queue_;
        queue_ = nullptr;
    }

    bool Create(USHORT size) {
        std::lock_guard<std::mutex> lock(mutex_);
        queue_ = new Type[size];
        bool result = (queue_ != nullptr);
        SecureZeroMemory(queue_, sizeof(Type) * size);
        head_pos_ = tail_pos_ = count_ = 0;
        size_ = size;
        return result;
    }

    /*  Test the queue is empty or not */
    bool IsEmpty() const {
        std::lock_guard<std::mutex> lock(mutex_);
        return count_ == 0;
    }

    /*  Test the queue is full or not */
    bool IsFull() const {
        std::lock_guard<std::mutex> lock(mutex_);
        return count_ == size_;
    }

    /* Reset the queue */
    void RemoveAll() {
        std::lock_guard<std::mutex> lock(mutex_);
        head_pos_ = tail_pos_ = count_ = 0;
        if (queue_ != nullptr) {
            SecureZeroMemory(queue_, sizeof(Type) * size_);
        }
    }

    /* Put an item into the queue */
    bool Put(const Type& item, bool overwrite = true) {
        std::lock_guard<std::mutex> lock(mutex_);
        if (overwrite) {
            queue_[tail_pos_++] = item;
            if (tail_pos_ == size_) {
                tail_pos_ = 0;
            }
            ++count_;
            if (count_ > size_) {
                count_ = size_;
            }
            return true;
        } else if (IsFull()) {
             return false;
        }
        return true;
    }

    /* Get an item out of the queue */
    Type Get() {
        std::lock_guard<std::mutex> lock(mutex_);
        USHORT temp_pos = head_pos_++;
        if (head_pos_ == size_) {
            head_pos_ = 0;
        }
        if (count_ > 0) {
            --count_;
        }
        return queue_[temp_pos];
    }

    /* Get the count of the items in the queue */
    USHORT GetCount() const {
        std::lock_guard<std::mutex> lock(mutex_);
        return count_;
    }

    USHORT GetSize() const {
        std::lock_guard<std::mutex> lock(mutex_);
        return size_;
    }
    /* Retrieve the index of the head entry */
    USHORT HeadPos() const {
        std::lock_guard<std::mutex> lock(mutex_);
        return head_pos_;
    }

    /* Retrieve the index of the tail entry */
    USHORT TailPos() const {
        std::lock_guard<std::mutex> lock(mutex_);
        return tail_pos_;
    }

    /* Get the last entry of the specified USHORT */
    USHORT LastEntry(USHORT current_entry) const {
        std::lock_guard<std::mutex> lock(mutex_);
        return (current_entry-- == 0) ? size_ - 1 : current_entry;
    }

    /* Get the next entry of the specified USHORT */
    USHORT NextEntry(USHORT current_entry) const {
        std::lock_guard<std::mutex> lock(mutex_);
        return (current_entry + 1 == size_) ? 0 : current_entry + 1;
    }

    /* Proceed the current USHORT to the next item */
    bool Proceed(USHORT& pos) {
        std::lock_guard<std::mutex> lock(mutex_);
        bool result = true;
        USHORT next_pos = (pos + 1 == size_) ? 0 : pos + 1;
        if (IsAnItem(pos)) {
            pos = next_pos;
        } else {
            result = false;
        }
        return result;
    }

    /* Retrieve the specified item (if not a member of the queue, return NULL) */
    Type* PeekItem(USHORT pos_item) {
        std::lock_guard<std::mutex> lock(mutex_);
        return (IsAnItem(pos_item)) ? &queue_[pos_item] : nullptr;
    }

    /* Add data to the ring buffer */
    bool AddData(Type* buffer, USHORT count_to_add) {
        std::lock_guard<std::mutex> lock(mutex_);
        if (count_ + count_to_add <= size_) {
            for (USHORT i = 0; i < count_to_add; ++i) {
                queue_[tail_pos_++] = buffer[i];
                if (tail_pos_ == size_) {
                tail_pos_ = 0;
                }
                ++count_;
            }
            return true;
        }
        return false;
    }

    /* Extract data from the ring buffer */
    bool ExtractData(Type* buffer, USHORT count_to_extract) {
        std::lock_guard<std::mutex> lock(mutex_);
        if (count_ >= count_to_extract) {
            for (USHORT i = 0; i < count_to_extract; ++i) {
                USHORT temp_pos = head_pos_++;
                if (head_pos_ == size_) {
                head_pos_ = 0;
                }
                --count_;
                buffer[i] = queue_[temp_pos];
            }
            return true;
        }
        return false;
    }
 private:
    bool IsAnItem(USHORT pos) const {
        if (count_ == 0) return false;
        if (pos < 0 || pos >= size_) return false;
        if (tail_pos_ == head_pos_) return (pos < size_);
        else if (tail_pos_ > head_pos_) return (pos >= head_pos_ && pos < tail_pos_);
        else return (pos < tail_pos_ || pos >= head_pos_);
    }

    USHORT size_;
    USHORT count_;
    USHORT head_pos_;
    USHORT tail_pos_;
    Type* queue_;
    mutable std::mutex mutex_;
};
